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Abstract — In this paper we consider the problem of high 
accuracy localization of mobile nodes in a multipath-rich 
environment where sub-meter accuracies are required. We 
employ a peer to peer framework where the vehicles/nodes 
can get pairwise multipath-degraded ranging estimates in 
local neighborhoods together with a fixed number of anchor 
nodes. The challenge is to overcome the multipath-barrier with 
redundancy in order to provide the desired accuracies especially 
under severe multipath conditions when the fraction of received 
signals corrupted by multipath is dominating. We invoke a 
message passing analytical framework based on particle filtering 
and reveal its high accuracy localization promise through 
simulations. 
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I. Introduction 

High-accuracy localization is mandated in many applica- 
tions like vehicle safety [1], autonomous robotic systems [2], 
Unmanned Air Vehicle (UAV) systems etc, where sub-meter 
accuracies are called for. Standard GPS receivers can have 
errors over fifty or more meters which is unacceptable for 
many of these applications. The principal problem is multipath 
1 interference [3], which is particularly prevalent in cities and 
"urban canyon" environments. In a multipath-rich environ- 
ment, the received signals are no longer gaussian in nature 
challenging the use of standard estimation techniques like the 
well-known Kalman Filtering framework and its extensions. 
The principal idea behind GPS is to obtain three or more 
distance measurements from sources with known locations 
(e.g. satellites) and estimate the location based on trilateration. 
However, even if one of the measurements is corrupted by 
multipath, the location errors can be significantly large. It has 
been well noted that redundancy in measurements is the key 
to tackle multipath [3]. Many of the existing solutions such 
as D-GPS, A-GPS [4] augment the GPS system by adding 
extra infrastructure in terms of fixed base stations. However 
infrastructure cost and complexity constrains the amount of 
redundancy that can be introduced in the system and as a 
consequence limits the achievable localization. 

In contrast to existing centralized systems that are based on 
a "cellular-like" architecture, with users individually comput- 
ing their location by calculating the distance to a small number 
of satellites and/or terrestrial base stations, we adopt a "peer 

Multiple delayed versions of the same transmitted signal are received at 
the receiver due to reflections from different objects in the environment. 
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Figure 1. Peer-to-Peer Collaborative Localization 

to peer" architecture (see Fig 1) where nodes collaborate and 
help each other to refine their position estimates. We look for 
distributed algorithms in the interest of scalability and reduced 
computational complexity. We propose the inclusion of low- 
cost static "anchor" nodes with known locations and study 
the effect of the number of anchor nodes and existing mobile 
nodes on the system performance. Collaboration coupled with 
mobility generates a large pool of measurements in the system. 
The fundamental insight is that, some fraction of these mea- 
surements will be produced by line-of-sight (LOS) dominated 
signals, and hence be fairly accurate, while some fraction will 
be corrupted by dominated non-line-of- sight (NLOS) reflected 
waves. Receivers do not know a priori which measurements 
are LOS and which are NLOS. Hence, the task of the users 
is to cooperatively discard the NLOS signals, thus enabling 
them to compute high-precision position estimates. Our main 
contribution in this paper is to uncover a framework and a 
distributed algorithm founded on message passing for collab- 
orative localization. We adopt a mixture model (discussed in 
Sec. II) for the received signal to characterize LOS/NLOS, that 
naturally arises given that the receivers do not know a priori 
the nature of the received signal. 

Distributed algorithms [2], [5] have been of recent interest 
for collaborative localization. However, most of the existing 
work in the literature on localization focus on the case when 
the measurements are gaussian. In our setting, the distributions 
of interest are mixture distributions that are highly non- 
gaussian in nature. Generalizations of Kalman Filtering, such 
as the Extended Kalman Filter (EKF) [6] have been proposed 
for non-gaussian problems. However, EKF solutions do not 
work well when the distributions are bimodal as in the case 
of mixture distributions, since gaussians do not approximate 
bimodal distributions very well. Some of the other approaches 
try to weed out the NLOS signals ( [3], [7], [8], [9]), and 
work only with the LOS signals which can be reasonably 
modeled as gaussian. The underlying algorithm used in most 
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of these approaches has been modified forms of the Random 
Sample Consensus (RANSAC) algorithm, which is a classical 
algorithm in computer vision literature to discard outliers in 
the data. RANSAC works well when the number of outliers 
(here the NLOS signals) is much smaller than the number 
of LOS signals. However, these are limited in settings (for 
e.g. see [10] for an ultra- wideband setting) where the fraction 
of LOS signals is typically lesser than NLOS which would 
challenge the performance of RANSAC-based approaches. 
The main reason for this is that in our case the NLOS signals 
dominate the set of measurements, and those can no longer 
be considered as outliers. Simulation results show that our 
algorithm works very well even in this challenging case where 
the fraction of LOS signals is dominated by the fraction of 
NLOS signals. An excellent survey of existing locationing 
algorithms and their drawbacks can be found in [11]. 

II. Problem Setup 

We have N mobile nodes with an arbitrary mobility model 
and M static anchor nodes with known locations. For simu- 
lations we consider the nodes to be moving with a constant 
velocity along a fixed trajectory, motivated by the highway 
setting for vehicles. Each vehicle is equipped with a sensor 
capable of getting a Time of Arrival (ToA)/ Time Difference 
of Arrival (TDoA) signal from other vehicles/anchors within 
a communication radius R. We model each measurement as 
either a LOS-dominated signal or an NLOS -dominated signal 
by choosing the observation noise in the received signal to 
be drawn from a mixture of two distributions, corresponding 
to LOS and NLOS respectively. The model is motivated by 
some of the experimental work carried out in the UWB [10], 
[12], [13]. For e.g., the experimental results in [10] show 
that some fraction of the received signals are purely LOS- 
dominated signals which motivates the mixture distribution. 
For simulations, we will model the noise in LOS as gaussian 
whereas for the NLOS we will take it to be a sum of an 
exponential and a gaussian distribution. The noise model for 
NLOS is again motivated from some of the experimental 
results discussed in [12], [13]. These show that the NLOS 
signal distribution is very close to an exponential distribution. 
This was earlier conjectured in [14], where the author argues 
that the arrivals of the different paths can be modeled as a 
Poisson process which in turn leads to the inter arrival times 
being exponential. The theory developed here is however, more 
generic and does not depend on the specific nature of the 
distributions. Each vehicle is assumed to be equipped with an 
accelerometer and magnetometer that give noisy readings of 
the velocity and direction of motion of the vehicle. We will 
assume that there is a multiple access protocol in place that 
will help the vehicles communicate across the shared medium. 

Let v t (k) denote the true location of the kth 
vehicle at time instant t. O t (km) denotes the reading 
(distance measurement) between the vehicle k and 
node 2 m at time instant t. The readings Q t (km) 

2 The term vehicle/anchor and node will be interchangeably used. 
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Figure 2. Graphical model representation of the unknown vehicle states 
(locations) and the observations (LOS/NLOS measurements). 

are sampled from a mixture of two distributions, 

(jpLOs{0t{km)\v t (k),v t (m)) 

with mixture probabilities (a, 1 — a) respectively. Let z t (km) 
be the indicator random variable for the LOS reading between 
vehicles k and m at time instant t. 

if O t (km) ~ PLOs(0t(krn)\vt(k),v t (rn)) 
otherwise 
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It is highly unlikely that a vehicle's state would change rapidly 
over time. To capture this, we will model the state evolutions 
of the z t (kmys as a "sticky" Markov Chain with a stationary 
distribution (a, 1 — a). Let p{v t {k)\v t -i{k)) be the distribution 
that governs the evolution of the vehicle states across time 
which is an artifact of the inertial navigation system. 

Every vehicle k needs to estimate its location v t (k), based 
on all measurements {0 T (km)} t r=1 from its neighbors upto 
time t. Given the non-gaussian nature of the problem and 
the accuracy requirement we need to develop robust, accurate 
and distributed algorithms. Particle Filters have gained impor- 
tance in the recent past for tackling non-gaussian estimation 
problems. These provide accuracies close to Minimum Mean 
Square Error (MMSE) estimates. The nature of our problem 
helps us obtain Kalman-like updates for particle filtering giv- 
ing rise to a simplified algorithm. A short primer on graphical 
models and particle filtering is provided in the next section. 

III. Graphical models & Particle Filtering Primer 

Graphical models and particle filtering have been exten- 
sively studied in the machine learning community. An excel- 
lent treatment of graphical models is provided in [15] and 
discussions on particle filtering can be found in [16]. We 
describe these briefly to make our paper self contained. 

A. Graphical models 

Graphical models provide a good way of understanding 
dependencies between different random variables. These mod- 
els come in handy when we are trying to estimate some 
parameters given probability distributions that have a certain 
structure and we need computationally feasible algorithms to 
get the estimates. A directed acyclic graph, (5(V, £), consists 
of a vertex set V and edge set £ which is the collection of 
all directed edges. The vertices (nodes) of the directed graph 
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1: Initialize: Sample {vq}^ 1 from p(vo), set Wq 

2: for t > 1 do 

3: Sample v\ from p(^t lul^). 
4: Update w\ = it;J_ 1 p(0t 
5: Normalize w\. 

6: Resample new set of particles and set w\ = 

7: end for 

8: Output:£[/(i;t)|0i:t] = E*=i 



represent random variables and the dependencies amongst the 
random variables are captured by the edges. Let {X s , s G V} 
denote the set of all random variables indexed by the nodes 
of the graph. For every node s G V, let tt s denote the set of 
indices of its parents 3 . For any S C V, let = {X s , s G 
Then, the joint probability distribution of the random variables 
can be factored as p(X v ) = Y\ seV p(X s \X 7rs ). 

Dependencies amongst the vehicle locations and the read- 
ings are captured by the graphical model shown in Fig 2, which 
is a coupled Hidden Markov Model (HMM). The unshaded 
nodes are the hidden nodes to be estimated 4 and the shaded 
nodes are observations coupling the different Markov chains 
of the vehicles, called as the evidence nodes. Based on the 
factorization described above, the joint probability distribution 
of the set of all random variables is given by equation (1). 

The primary goal here is to estimate the hidden nodes 
given the observations. It is hard to directly apply conven- 
tional inference algorithms such as the celebrated loopy belief 
propagation [15] considering the fact that the hidden states are 
continuous in nature and that the graph grows over time. We 
will introduce approximations to the graphical model and use 
particle filtering to do inference over the approximated model. 
Simulation results show that we can obtain high accuracies in 
spite of the approximations. We briefly describe particle filters 
in the next section. 

B. Particle Filtering 

Particle Filtering is a Monte Carlo simulation technique 
whose goal is to approximate the posterior state density that 
can be used to obtain the MMSE estimate. We will consider 
only a single vehicle for now and omit the vehicle index 
k for notational simplicity. Let v\. t and 0i :t denote the set 
of states and observations up to time t respectively. The 
posterior density of the state given the observations can be 
approximated as p(v t \0 1:t ) « p(v t \0 1:t ) = Yn=i K v t ~ *4) 
where {v\;i = are K i.i.d random samples (parti- 

cles) picked from the distribution p(v t \0i :t ) and S(.) is the 
dirac-delta function. Expectations of some functions of the 
state f(v t ), can then be approximated with high probability, 
for large K, as E[f(v t )\e 1:t ] « /(«?)• Typically 

sampling from the true posterior distribution is hard. Let us 
consider sampling from a distribution 7r(vi :t \0i :t ), known as 
proposal distribution. After some simple manipulations using 

3 Node p is the parent of node s, if there is an incoming edge to s from p. 
4 Anchor nodes are not shown in this model for simplicity. 
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Bayes rule, we get 

E[f(v 1:t )\e 1:t ] 



En [Wt(Vl:t)f(Vl:t)] 
K[Wt(vi:t)] 



where w t (vi :t ) are known as the importance weights given by 



W t (Vi :t 



p(9l:t\Vl:t)p(vi:t) 

n(v 1: t\0i:t) 



Thus, if we have i.i.d samples of v\ :t from the proposal distri- 
bution 7r(vi : t\9i:t), we get E[f(v 1:t )\0 1:t ] « £) i=1 w t l f(v[ :t ), 
where w t l are the normalized importance weights. Conver- 
gence of this estimate to the true MMSE estimate is shown in 
[16]. 

For the case of a single HMM (Fig 2), we have 

P(vi:u0l:t) = P(^l)U]=2P( V 3\ V 3-l)U] = lP( j\ v j)' LetS 

choose a proposal distribution that admits a decomposition 

n(vi:t\0i:t) = n(v 1: t-i\0 1: t-i)7r(v t \v 1: t-i0 1: t). After simpli- 
fications, we can obtain a recursive estimation for the weights, 



w t -ip(Ot\vt)p(vt\v t - 



n(v t \vi:t-lOl:t) 

The optimal proposal distribution, that minimizes the variance 
of the error has been shown [16] to be p(v t \v t -i, 6 t ). Since 
sampling from this distribution is difficult for our problem, we 
will use a simpler distribution 7r(v t \vi :t -i0i : t) — p(v t \v t -i) 
to get the weight updations w t = w t -ip(O t \v t ). The reader 
is referred to the literature for other methods of choosing a 
proposal distribution. Typically a resampling step is introduced 
(see Alg. 1), to handle degeneracy issues when particle weights 
become too low. 

IV. Particle Filtering for Localization 

Exact inference over Fig 2 being hard, we will resort to an 
approximation for every vehicle as shown in Fig 3. The new set 
of shaded nodes in this graph correspond to the estimated lo- 
cation of the other vehicles. At every time instant each vehicle 
gets the estimated location of its neighbors from the previous 
time instants and assuming that it is close enough to the true 
location, the vehicle gets an estimate of its own location using 
particle filtering. A straightforward method of particle filtering 
over this model would be to consider {v t: z t } as a random 
variable pair which would reduce the graphical model to a 
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P(i v t}, {0 t }, {zt}) = Yl P(0t(km)\v t (k), v t (m), z t (km)) Y[ p(vi(k))Y[p(vt(k)\v t -i(k) Y[ p(z 1 (km))J^p(z t (km)\z t -i(km)). 
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Cumulative Density Function of the Localization Error 



Algorithm 2 Particle Filtering for Accurate Localization 

1: Initialize: Sample {^o(&)}iLi fr° m P( v o(k)), set Wg(k) = V/c. 
2: for t > 1 do 

3: Sample v\(k) from p(vt(k)\v\_ 1 (k)). 

4: for all Neighbors m do 

5: if m was a neighbor at t — 1 then 

6: <pl(km) = p(9 t (km)\zt(km) , vl(k)) 

^*t_i 2 ? ( 2; t( /cm )l 2; t-i( /cm )) ( / > t-i( /cm )- 

7: else 

8: <pl(km) = p(zt(km))p(9t(km)\v\(k) , zt(km)). 

9: end if 

10: end for 

11: Calculate w\{k) = Yl m E Zt ( fcm ) <^0™) 
12: Normalize u>J and resample. 
13: end for 

14: Output:£[vt(fc)|{0i : t(fcm)}] = Eili ™t (&mK (fc). 

simple HMM. However the state space of the random variables 
grows exponentially with the number of neighbors as we now 
need to sample particles over {v t (k), z t (km)}, which could 
lead to scaling issues. More importantly particle filtering is 
efficient when the hidden states are continuous, whereas z t 's 
are binary. Thus we now see to combine particle filtering and 
exact inference to obtain simplified weight updations. 

Consider the HMM in Fig 3 and ignore the vehicle indices 
k and m. Using the same definition of w t (vi :t ) as before, we 
can now write w t (v 1:t ) = Y, Zt <f>(v 1:t , z t ), where 

p(0l:t, Zt\vi:t)p(vi:t) 
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(a) Cumulative density function of localization error as a function of a. 
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Assuming a similar proposal distribution factorization as be- 
fore we can show that 



</>(vi. mt ,Zt) 



p(Ot\zt,v t )p(v t \vt-i) 



Zi|^_i)0(i;i : t_i,St_i). 
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n(Vt\vi:t-lOl:t) 

Choosing 7r(v t \v 1: t-i0 1:t ) =p(v t \v t -i), we get 

<t>t — <j>(vi :t ,Zt) = p(6 t \z U V t ) ^ p(Zt\Zt-l)(/)(vi:t-l, Z t -l). 

z t -i 

For each vehicle k and its neighbor m we have <p t (km) and 
the update equations are given in the Algorithm 2. 
The optimal detection rule for z t is given by 

" 1 if p(z t = l\0 1:t ) > p(z t = 0\0 1:t ) 
otherwise 



Horizontal Axis of Motion 
(b) True and estimated vehicle trajectories for the proposed algorithm, a = 15% 
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(c) Mean estimation errors and the detection probability of the LOS signals 
(Pd) as a function of a. 



This can be simplified and shown to be equal to the following 
test E^(j>{v 1:U z t = 1) En<i>(vi:t,zt = 0), which is 
evaluated by approximating E^^vi-t, z t ) ~ J2 i <j>(v\. t ^ z t ). To 
take care of degeneracy issues over long time instants [16], we 
reset the system when the number of distinct particles become 
small and sample a new set of particles from a neighborhood 
around the location estimate at that time instant. 

V. Simulation results 

The simulation set up consists of N = 20 vehicles moving 
in a grid of size 150m x 30m. The vehicles start at random 
locations from the left most corner of the grid and move at 
a constant velocity to the right. To account for the vertical 
motion, a simplistic model of a curved trajectory is simulated 
for each vehicle. The observations are generated as follows. 
If d t (km) = \ \v t (k) - v t {m)\ \ < R, 



Num of Anchors 
(M) 


Num of Vehicles 
(N) 


Mean Error 
(Particle Filtering) 


26 


20 


0.93 m 


26 


24 


0.82 m 


26 


30 


0.76 m 


30 


20 


0.74 m 


36 


20 


0.70 m 



t(km) 



d t (km) + n t (km) if LOS 
d t (km) + e t (km) + n t (km) otherwise 



(d) Mean estimation errors of the particle filtering algorithm as a function of 
the number of anchor nodes and vehicles for a = 30%. 

Figure 4. Parameters for all the plots: &ins — 0-l> ^LOS — 0.05, 
® N LOS = 5, Num of particles = 900, Horizontal velocity^ 0.2m per 
time step V/c, M = 26, iV = 20, R = 10m. The plots are a function of 
the fraction of LOS signals a. The proposed particle filtering algorithm is 
compared against the local Maximum Likelihood algorithm. 

where n t (km) ~ ^(^^los) i-i-d, e t (km) ~ Exp(a^ 1 LOS ). 
We assume the fraction of the readings that are LOS to be a. 
The evolution of the z t (km) random variables is governed by 
the probability law, p(z t (km) = l\z t -i(km) = 0) = f for 
these simulations and the other values are taken so that the 
stationary distribution is (a, 1 — a). The inertial navigation 



system readings are assumed to be obtained under an additive 
white gaussian noise model. Time steps are divided into units 
of one for simplicity. We compare the performance of the 
algorithm to a local genie aided Maximum Likelihood (ML) 
algorithm. Here each vehicle, at every time instant, calculates 
the local ML estimate of its location assuming that a genie 
provides it with the exact locations of its neighbors. 

The localization error cumulative density function is plotted 
for different values of a in Fig 4(a). The x-axis is the set 
of error values and ?/-axis is the cumulative density function. 
One can see that even at low values of a = 5%, more 
than 80% of the errors are less than 1.5m. The algorithm 
performance is slightly better than ML algorithm. This is 
not too surprising considering that particle filtering tries to 
approximate MMSE which is the optimal solution for mean 
squared loss function. Algorithms based on RANSAC were 
found to have errors over 5m and are not discussed here. 
The true and estimated vehicle trajectories are plotted in Fig 
4(b). The estimated vehicle trajectory is generated by a simple 
polynomial fit to all the estimated vehicle locations. Table 4(c) 
shows the probability of detection (P^) of the hidden z t (km) 
states (fraction of the times LOS states are detected correctly) 
and the mean estimation error. The strength of the algorithm 
is in exploiting the large pool of measurements efficiently 
where algorithms like RANSAC fail. Table 4(d) shows the 
localization error performance as a function of the number 
of anchor nodes and mobile nodes in the system. One can 
observe a law of diminishing returns as the number of anchor 
nodes increases. Another interesting observation is that the 
percentage improvement in the localization performance when 
the number of mobile nodes, that act like "pseudo-anchors", 
is the similar to that of adding more anchors. A theoretical 
exploration of this phenomenon is currently under progress. 

We shall now discuss some practical aspects of implement- 
ing this scheme. The communication between the nodes can 
be carried over the Dedicated Short Range Communications 
(DSRC) band, that has been allocated by the Federal Com- 
munications Commission (FCC) for Intelligent Transportation 
Systems. IEEE 802. lip can be used for medium access. 
This is essentially a CSMA protocol specialized for vehicular 
networks. Whenever a node receives a 802. lip packet from 
some other node, it can estimate the time delay of arrival that 
translates to a distance measurement used for localization. 
The location estimate sent by one node to the other node 
can be sent as part of the 802. lip packet. The parameters 
of the distributions and the fraction a can be estimated using 
an Expectation Maximization (EM) algorithm at every step. 
Each vehicle has an average of 8 other vehicles and 4 anchor 
nodes in its communication radius which is quite a reasonable 
assumption. In practice, the anchors usually have a larger range 
of communication and the anchor density required would be 
lesser than that used in the simulations. We assume that the 
computational load in updating the particles is manageable by 
present day vehicles that have a reasonably high processing 
power. 



VI. Conclusion and Future work 

In this work we explored the application of particle filtering 
to get estimates of vehicle locations in a highly NLOS 
environment. We derived weight update equations for the 
NLOS setting and simulation results show that reasonably 
good accuracies in positioning is feasible. Future work in- 
cludes carrying out more realistic simulations using traffic and 
network simulators. The approximation in the graphical model 
could break down above a certain noise threshold and below 
a certain anchor density, and the algorithm could potentially 
diverge. A theoretical understanding of when the algorithm 
diverges is another research direction, though we believe this 
to be a hard problem. A theoretical exploration of the effect 
of increased anchor and mobile nodes is under progress. 
Integrating other sensing modalities into the algorithm is a 
future research direction. 
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